
#include "unitree_legged_sdk/unitree_legged_sdk.h"

using namespace UNITREE_LEGGED_SDK;

class Robot
{
public:
    Robot(uint8_t level=HIGHLEVEL): 
      safe(LeggedType::Go1), 
      udp(level, 8090, "192.168.123.161", 8082){
        udp.InitCmdData(cmd);
        init_cmd();
    }
    void init_cmd();
    void control();
    void getstate();

    Safety safe;
    UDP udp;
    HighCmd cmd = {0};
    HighState state = {0};
};


void Robot::init_cmd()
{
    cmd.mode = 0;      // 0:idle, default stand      1:forced stand     2:walk continuously
    cmd.gaitType = 0;
    cmd.speedLevel = 0;
    cmd.footRaiseHeight = 0;
    cmd.bodyHeight = 0;
    cmd.euler[0]  = 0;
    cmd.euler[1] = 0;
    cmd.euler[2] = 0;
    cmd.velocity[0] = 0.0f;
    cmd.velocity[1] = 0.0f;
    cmd.yawSpeed = 0.0f;
    cmd.reserve = 0;
    return;
}


void Robot::control()
{
    udp.SetSend(cmd);
    udp.Send();
    init_cmd();
    return;
}

void Robot::getstate()
{
    udp.Recv();
    udp.GetRecv(state);
    return;
}